 #include <QWheelEvent>
#include <QGraphicsSceneMouseEvent>
#include "mygraphicsscene.h"
#include "mygoalitem.h"
#include <math.h>

#define PI 3.14159265

myGraphicsScene::myGraphicsScene(QObject*parent,ArClientBase *client):
    QGraphicsScene(parent)
{

    sendRobot = false;
    sendingRobot = false;
    handleclient = client;
}

void myGraphicsScene::wheelEvent(QGraphicsSceneWheelEvent *e){

}


void myGraphicsScene::setSendRobot(bool value){
    this->sendRobot = value;
    if(value && locateRobot) locateRobot = false;
}

void myGraphicsScene::setLocateRobot(bool value){
    this->locateRobot = value;
    if(value && sendRobot) sendRobot = false;
}

void myGraphicsScene::mousePressEvent(QGraphicsSceneMouseEvent *e){

    switch(e->button()){

        case Qt::LeftButton:
            printf("Pulsado boton izquierdo\n");

            if(sendRobot){
                sendingRobot = true;

                int X,Y;

                X = e->scenePos().x();
                Y = (e->scenePos().y())*(-1);

                sendPose = new ArPose(X,Y,0);
                posSend = new myGoalItem(Qt::gray,QString(""));
                posSend->setX(X);
                posSend->setY(Y*(-1));
                posSend->setRotation(0);


                emit signal_paint_goal(posSend);

            }else if(locateRobot){
                locatingRobot = true;

                int X,Y;

                X = e->scenePos().x();
                Y = (e->scenePos().y())*(-1);

                sendPose = new ArPose(X,Y,0);
                posSend = new myGoalItem(Qt::gray,QString(""));
                posSend->setX(X);
                posSend->setY(Y*(-1));
                posSend->setRotation(0);


                emit signal_paint_goal(posSend);
            }

            break;

    case Qt::RightButton:
        printf("Pulsado boton derecho\n");
        break;

    case Qt::MiddleButton:
        printf("Pulsado boton central\n");
        break;

    case Qt::NoButton:
        printf("No button\n");
        break;

    }

}

void myGraphicsScene::mouseReleaseEvent(QGraphicsSceneMouseEvent *e){

    switch(e->button()){

        case Qt::LeftButton:

        if(sendRobot && sendingRobot){

            ArNetPacket p;

            p.byte4ToBuf(sendPose->getX());
            p.byte4ToBuf(sendPose->getY());
            p.byte4ToBuf(sendPose->getTh());

            printf("Enviando a: x: %f y: %f\n",sendPose->getX(),sendPose->getY());

            handleclient->requestOnce("gotoPose",&p);
            emit signal_remove_goal(posSend);
            sendingRobot = false;

        }else if(locateRobot && locatingRobot){

            ArNetPacket p;

            p.byte4ToBuf(sendPose->getX());
            p.byte4ToBuf(sendPose->getY());
            p.byte4ToBuf(sendPose->getTh());

            printf("Enviando a: x: %f y: %f\n",sendPose->getX(),sendPose->getY());

            handleclient->requestOnce("localizeToPose",&p);
            emit signal_remove_goal(posSend);
            locatingRobot = false;

        }


            printf("Soltado boton izquierdo\n");
            break;

    case Qt::RightButton:
        printf("Soltado boton derecho\n");
        break;

    case Qt::MiddleButton:
        printf("Soltado boton central\n");
        break;

    case Qt::NoButton:
        printf("No button Soltado\n");
        break;

    }

}

void myGraphicsScene::mouseMoveEvent(QGraphicsSceneMouseEvent *event){


    if(sendingRobot || locatingRobot){



        double theta = 0,tan,a,b;


        if(event->scenePos().x() == posSend->x() && event->scenePos().y() > posSend->y())
            theta = 180;
        else if(event->scenePos().x() == posSend->x() && event->scenePos().y() < posSend->y())
            theta = 0;
        else if(event->scenePos().x() > posSend->x() && event->scenePos().y() == posSend->y())
            theta = 90;
        else if(event->scenePos().x() < posSend->x() && event->scenePos().y() == posSend->y())
            theta = 270;
        else if(event->scenePos().x() > posSend->x() && event->scenePos().y() < posSend->y())
        {

            a = abs(event->scenePos().x() - posSend->x());
            b = abs(posSend->y() - event->scenePos().y());


            tan = a/b;

            theta = atan(tan) * 180.0 / PI;

        }
        else if(event->scenePos().x() > posSend->x() && event->scenePos().y() > posSend->y())
        {

            b = abs(event->scenePos().x() - posSend->x());
            a = abs(event->scenePos().y() - posSend->y());


            tan = a/b;

            theta = 90.0 + (atan(tan) * 180.0 / PI);

        }
        else if(event->scenePos().x() < posSend->x() && event->scenePos().y() > posSend->y())
        {

            a = abs(posSend->x()-event->scenePos().x());
            b = abs(event->scenePos().y() - posSend->y());


            tan = a/b;

            theta = 180.0 + (atan(tan) * 180.0 / PI);

        }
        else if(event->scenePos().x() < posSend->x() && event->scenePos().y() < posSend->y())
        {

            b = abs(posSend->x() - event->scenePos().x());
            a = abs(posSend->y() - event->scenePos().y());


            tan = a/b;

            theta = 270.0 + (atan(tan) * 180.0 / PI);

        }



        posSend->setRotation(theta);

        if(theta>180)theta = 550-theta;
        else theta = 180 - theta;

        theta = ((int)theta+270) % 360;

        sendPose->setTh(theta);



    }





}
